#include "Physic_World.h"

Physic_Physic::Physic_Physic(){};

Physic_Physic::~Physic_Physic(){};

void Physic_Physic::Update(Physic_Object *a_Physic_Object, const float a_time){
	Vector3 position, velocity;
	float angle, maxSpeed, drag, dragTime;
	position = a_Physic_Object->getPosition();
	velocity = a_Physic_Object->getVelocity();
	angle = a_Physic_Object->getAngle();
	maxSpeed = a_Physic_Object->getMaxSpeed();
	drag = a_Physic_Object->getDrag();
	dragTime = a_Physic_Object->getDrag()*a_time;
	float mag = velocity.magnitude();
	if(mag <= 0.0f){
		return;
	}
	if(mag > maxSpeed){
		velocity.normalize();
		velocity.multiply(maxSpeed);
		mag = velocity.magnitude();
	}

	Vector3 tempDrag = velocity.normal();
	tempDrag.multiply(-1.0f*(dragTime));
	float dragV = tempDrag.magnitude();
	if(mag > dragV){
		velocity.add(tempDrag);
		mag = velocity.magnitude();
	}else{
		velocity.multiply(0.0f);
	}

	a_Physic_Object->setVelocity(velocity.x, velocity.y, velocity.z);

	Vector3 tempVec = velocity;
	Vector3 origin = Vector3(0, 0, 0);
	Vector3 tempUpV = origin;
	tempUpV.z = origin.z + 1;
	Vector3 tempRightV = origin;
	tempRightV.x = origin.x + 1;

	if(tempVec.dotProduct(tempUpV) < 0.0f){
		tempRightV.x = origin.x - 1;
	}
	tempVec.subtract(origin);
	tempRightV.subtract(origin);
	tempVec.normalize();
	tempRightV.normalize();
	float dot = tempVec.dotProduct(tempRightV);
	float length = tempVec.magnitude()*tempRightV.magnitude();
	float PI = 3.14159258f;
	float theta = 0.0f;
	if(length != 0.0f){
		theta = acos(dot/length);
		if(tempVec.dotProduct(tempUpV) < 0.0f){
			theta += PI;
		}
	}else{
		theta = angle;
	}
	angle = theta;
	a_Physic_Object->setAngle(angle);
	float angleDegree = angle * ((180 / PI));

	velocity.multiply(a_time);
	position.add(velocity);
	a_Physic_Object->setPosition(position.x, position.y, position.z);
};

Physic_Contact::Physic_Contact(){};

Physic_Contact::~Physic_Contact(){};

void Physic_Contact::contact(Physic_Object *a_Physic_Object_To_Resolve, Physic_Object *a_Physic_Object_To_Resolve_With){
	Vector3 tempPositionOfA = a_Physic_Object_To_Resolve->getPosition();
	Vector3 tempPositionOfB = a_Physic_Object_To_Resolve_With->getPosition();

	float tempDistanceAB = tempPositionOfA.distance3D(tempPositionOfB);

	if(tempDistanceAB <= (a_Physic_Object_To_Resolve->getRadius()+a_Physic_Object_To_Resolve_With->getRadius())){
		for(int i  = 0; i < 10; i++){
			if(a_Physic_Object_To_Resolve->m_contact[i] != a_Physic_Object_To_Resolve_With 
				&& a_Physic_Object_To_Resolve->m_contact[i] == 0){
					a_Physic_Object_To_Resolve->m_contact[i] = a_Physic_Object_To_Resolve_With;
			}
			if(a_Physic_Object_To_Resolve->m_contact[i] == a_Physic_Object_To_Resolve_With){
				Vector3 temV = a_Physic_Object_To_Resolve->getVelocity();
				float temVMag = temV.magnitude();
				//if(temVMag > 0.0f){
					Vector3 tempD = tempPositionOfA.minus(tempPositionOfB);
					float tempMag = tempD.magnitude();
					tempD.normalize();
					temV.add(tempD);
					//a_Physic_Object_To_Resolve->setVelocity(temV.x, temV.y, temV.z);
					float depth = (a_Physic_Object_To_Resolve->getRadius() + a_Physic_Object_To_Resolve_With->getRadius()) - tempMag;
					//tempD.normalize();
					tempD.multiply(depth);
					//a_Physic_Object_To_Resolve->setPosition(tempPositionOfA.x+tempD.x, tempPositionOfA.y+tempD.y, tempPositionOfA.z+tempD.z);
				//}
				break;
			}
		}
	}else{
		for(int i  = 0; i < 10; i++){
			if(a_Physic_Object_To_Resolve->m_contact[i] == a_Physic_Object_To_Resolve_With){
				a_Physic_Object_To_Resolve->m_contact[i] = 0;
				break;
			}
		}
	}
};

Physic_Object::Physic_Object(){
	m_contact = new Physic_Object*[10];
	for(int i = 0; i < 10; i++){
		m_contact[i] = 0;
	}
	m_position = Vector3(0.0f, 0.0f, 0.0f);
	m_velocity = Vector3(0.0f, 0.0f, 0.0f);
	m_radius = 0.0f;
	m_angle = 0.0f;
	m_maxSpeed = 0.0f;
	m_drag = 0.0f;
};

Physic_Object::Physic_Object(const Physic_Object_Frame *a_Physic_World_Object_Frame){
	m_contact = new Physic_Object*[10];
	for(int i = 0; i < 10; i++){
		m_contact[i] = 0;
	}
	m_position = a_Physic_World_Object_Frame->m_position;
	m_velocity = a_Physic_World_Object_Frame->m_velocity;	
	m_radius = a_Physic_World_Object_Frame->m_radius;
	m_angle = a_Physic_World_Object_Frame->m_angle;
	m_maxSpeed = a_Physic_World_Object_Frame->m_maxSpeed;
	m_drag = a_Physic_World_Object_Frame->m_drag;
};

Physic_Object::~Physic_Object(){
	// causes stack overflow
	//if(m_contact){
	//	for(int i = 0; i < 10; i++){
	//		delete m_contact[i];
	//		m_contact[i] = 0;
	//	}
	delete m_contact;
	m_contact = 0;
	//}
};

bool Physic_Object::collide(){
	if(m_contact == 0){
		return false;
	}
	for(int i = 0; i < 10; i++){
		if(m_contact[i] != 0){
			return true;
		}
	}
	return false;
};

bool Physic_Object::collide(Physic_Object *a_Physic_Object){
	if(a_Physic_Object == 0){
		return false;
	}
	for(int i = 0; i < 10; i++){
		if(m_contact[i] != 0){
			if(m_contact[i] == a_Physic_Object){
				return true;
			}
		}
	}
	return false;
};

void Physic_Object::setPosition(float a_x, float a_y, float a_z){
	m_position.x = a_x;
	m_position.y = a_y;
	m_position.z = a_z;
};

const Vector3 Physic_Object::getPosition(){
	return m_position;
};

void Physic_Object::setVelocity(float a_x, float a_y, float a_z){
	m_velocity.x = a_x;
	m_velocity.y = a_y;
	m_velocity.z = a_z;
};

const Vector3 Physic_Object::getVelocity(){
	return m_velocity;
};

void Physic_Object::addVelocity(float a_x, float a_y, float a_z){
	Vector3 tempVec3(a_x, a_y, a_z);
	m_velocity.add(tempVec3);
};

void Physic_Object::setRadius(float a_r){
	m_radius = a_r;
};

const float Physic_Object::getRadius(){
	return m_radius;
};

void Physic_Object::addRadius(float a_r){
	m_radius += a_r;
};

void Physic_Object::setAngle(float a_angle){
	m_angle = a_angle;
};
void Physic_Object::addAngle(float a_angle){
	m_angle += a_angle;
};
const float Physic_Object::getAngle(){
	return m_angle;
};

void Physic_Object::setMaxSpeed(float a_maxSpeed){
	m_maxSpeed = a_maxSpeed;
};

const float Physic_Object::getMaxSpeed(){
	return m_maxSpeed;
};

void Physic_Object::setDrag(float a_drag){
	m_drag = a_drag;
};

const float Physic_Object::getDrag(){
	return m_drag;
};

Physic_World::Physic_World(){};
Physic_World::~Physic_World(){};

void Physic_World::Shutdown(){
	m_Physic_Object_List.clearAll();
};

void Physic_World::SetPhysic(const float a_x, const float a_y, const float a_z){
};

Physic_Object *Physic_World::CreateObject(const Physic_Object_Frame *a_Physic_World_Object_Frame){
	Physic_Object *temp_Physic_Object = 0;

	if(m_Physic_Object_List.getSizeUsed() == 0 || m_Physic_Object_List.getSizeUsed() == m_Physic_Object_List.getSizeCapacity()){
		temp_Physic_Object = new Physic_Object(a_Physic_World_Object_Frame);

		m_Physic_Object_List.insertToEnd(temp_Physic_Object);
		temp_Physic_Object = m_Physic_Object_List.getIndex(m_Physic_Object_List.getSizeUsed()-1);
	}else{
		for(int i = 0; i < m_Physic_Object_List.getSizeCapacity(); i++){
			if(m_Physic_Object_List.getIndex(i) == 0){
				temp_Physic_Object = new Physic_Object(a_Physic_World_Object_Frame);

				if(!m_Physic_Object_List.insertToIndex(i, temp_Physic_Object)){
					m_Physic_Object_List.insertToEnd(temp_Physic_Object);
				}

				temp_Physic_Object = m_Physic_Object_List.getIndex(m_Physic_Object_List.getSizeUsed()-1);
				break;
			}
		}
	}

	return temp_Physic_Object;
};

void Physic_World::DestroyObject(Physic_Object *a_Physic_World_Object){
	for(int i = 0; i < m_Physic_Object_List.getSizeUsed(); i++){
		if(m_Physic_Object_List.getIndex(i) == a_Physic_World_Object){
			m_Physic_Object_List.removeFromIndex(i);
			break;
		}
	}
};

void Physic_World::Update(const float a_time){
	Physic_Object *tempObjA, *tempObjB;
	tempObjA = tempObjB = 0;
	for(int i = 0; i < m_Physic_Object_List.getSizeUsed(); i++){
		tempObjA = m_Physic_Object_List.getIndex(i);
		if(tempObjA != 0){
			m_Physic_Physic.Update(tempObjA, a_time);

			for(int j = 0; j < m_Physic_Object_List.getSizeUsed(); j++){
				tempObjB = m_Physic_Object_List.getIndex(j);
				if(tempObjB!= 0 && tempObjB != tempObjA){
					m_Physic_Contact.contact(tempObjA, tempObjB);
				}
			}
		}
	}
};
